﻿#include "OiPCH.hpp"
#include <QtConcurrent/QtConcurrentRun>
#include "Geom/OiCgal.hpp"
#include "CGAL/linear_least_squares_fitting_2.h"
#include "Math/OiOptimize.hpp"
#include <xlsxdocument.h>
namespace Oi
{
    namespace PLScannerCore
    {
        inline int suitGap(const cv::Point2f& s1, const cv::Point2f& s2, const cv::Point2f& t1, const cv::Point2f& t2,
            const cv::Mat& homoB, const cv::Mat& homo1, const cv::Mat& homo2)
        {
            float rx = s1.x;
            float ry = s1.y;
            float denum = homoB.at<double>(2, 0) * rx + homoB.at<double>(2, 1) * ry + homoB.at<double>(2, 2);
            float homox = (homoB.at<double>(0, 0) * rx + homoB.at<double>(0, 1) * ry + homoB.at<double>(0, 2)) / denum;
            float homoy = (homoB.at<double>(1, 0) * rx + homoB.at<double>(1, 1) * ry + homoB.at<double>(1, 2)) / denum;
            rx = homox;
            ry = homoy;
            denum = homo1.at<double>(2, 0) * rx + homo1.at<double>(2, 1) * ry + homo1.at<double>(2, 2);
            float x = (homo1.at<double>(0, 0) * rx + homo1.at<double>(0, 1) * ry + homo1.at<double>(0, 2)) / denum;
            float y = (homo1.at<double>(1, 0) * rx + homo1.at<double>(1, 1) * ry + homo1.at<double>(1, 2)) / denum;
            Math::Point2f homo1S1(x, y);
            rx = homox;
            ry = homoy;
            denum = homo2.at<double>(2, 0) * rx + homo2.at<double>(2, 1) * ry + homo2.at<double>(2, 2);
            x = (homo2.at<double>(0, 0) * rx + homo2.at<double>(0, 1) * ry + homo2.at<double>(0, 2)) / denum;
            y = (homo2.at<double>(1, 0) * rx + homo2.at<double>(1, 1) * ry + homo2.at<double>(1, 2)) / denum;
            Math::Point2f homo2S1(x, y);

            rx = s2.x;
            ry = s2.y;
            denum = homoB.at<double>(2, 0) * rx + homoB.at<double>(2, 1) * ry + homoB.at<double>(2, 2);
            homox = (homoB.at<double>(0, 0) * rx + homoB.at<double>(0, 1) * ry + homoB.at<double>(0, 2)) / denum;
            homoy = (homoB.at<double>(1, 0) * rx + homoB.at<double>(1, 1) * ry + homoB.at<double>(1, 2)) / denum;
            rx = homox;
            ry = homoy;
            denum = homo1.at<double>(2, 0) * rx + homo1.at<double>(2, 1) * ry + homo1.at<double>(2, 2);
            x = (homo1.at<double>(0, 0) * rx + homo1.at<double>(0, 1) * ry + homo1.at<double>(0, 2)) / denum;
            y = (homo1.at<double>(1, 0) * rx + homo1.at<double>(1, 1) * ry + homo1.at<double>(1, 2)) / denum;
            Math::Point2f homo1S2(x, y);
            rx = homox;
            ry = homoy;
            denum = homo2.at<double>(2, 0) * rx + homo2.at<double>(2, 1) * ry + homo2.at<double>(2, 2);
            x = (homo2.at<double>(0, 0) * rx + homo2.at<double>(0, 1) * ry + homo2.at<double>(0, 2)) / denum;
            y = (homo2.at<double>(1, 0) * rx + homo2.at<double>(1, 1) * ry + homo2.at<double>(1, 2)) / denum;
            Math::Point2f homo2S2(x, y);

            float homo1Dis1 = (homo1S1 - Math::Point2f(t1.x, t1.y)).norm();
            float homo2Dis1 = (homo2S1 - Math::Point2f(t1.x, t1.y)).norm();
            float homo1Dis2 = (homo1S2 - Math::Point2f(t2.x, t2.y)).norm();
            float homo2Dis2 = (homo2S2 - Math::Point2f(t2.x, t2.y)).norm();
            if (homo1Dis1 < homo2Dis1 && homo1Dis2 < homo2Dis2)
            {
                return s2.x + 30;
            }
            else if (homo1Dis1 > homo2Dis1 && homo1Dis2 > homo2Dis2)
            {
                return s1.x - 30;
            }
            else if (homo1Dis1 < homo2Dis1 && homo1Dis2 > homo2Dis2)
            {
                return (s1.x + s2.x) / 2;
            }
            else
            {
                if (homo1Dis1 + homo1Dis2 < homo2Dis1 + homo2Dis2)
                {
                    return s1.x - 30;
                }
                else
                {
                    return s2.x + 30;
                }
            }

            return 0;
        }

        inline bool extractSawtoothPoints(const Base::List<Math::Point2d>& laserLine, std::vector<Math::Point2d>& extractedPoints, int index)
        {
            int minIndex = 0;
            float minLaser = 2048;
            for (int i = 0; i < laserLine.size(); i++)
            {
                if (laserLine[i].y() < minLaser)
                {
                    minLaser = laserLine[i].y();
                    minIndex = i;
                }
            }

            std::vector<cv::Point2f> peakPoses;
            peakPoses.push_back(cv::Point2f(laserLine[minIndex].x(), laserLine[minIndex].y()));

            int gap = 280;
            int startIndex = minIndex + gap / 2;
            while (peakPoses.size() < 6 && startIndex + gap < laserLine.size())
            {
                int currMinIndex = 0;
                minLaser = 2048;
                for (int i = startIndex; i < startIndex + gap; i++)
                {
                    if (laserLine[i].y() < minLaser)
                    {
                        minLaser = laserLine[i].y();
                        currMinIndex = i;
                    }
                }

                if (qAbs(laserLine[currMinIndex].y() - peakPoses.back().y) > 10)
                {
                    break;
                }

                peakPoses.push_back(cv::Point2f(laserLine[currMinIndex].x(), laserLine[currMinIndex].y()));
                startIndex = currMinIndex + gap / 2;
            }

            startIndex = minIndex - gap / 2;
            while (peakPoses.size() < 6 && startIndex - gap > 0)
            {
                int currMinIndex = 0;
                minLaser = 2048;
                for (int i = startIndex; i > startIndex - gap; i--)
                {
                    if (laserLine[i].y() < minLaser)
                    {
                        minLaser = laserLine[i].y();
                        currMinIndex = i;
                    }
                }

                if (qAbs(laserLine[currMinIndex].y() - peakPoses.front().y) > 10)
                {
                    break;
                }

                peakPoses.insert(peakPoses.begin(), cv::Point2f(laserLine[currMinIndex].x(), laserLine[currMinIndex].y()));
                startIndex = currMinIndex - gap / 2;
            }

            if (peakPoses.size() != 6)
            {
                return false;
            }

            for (int i = 0; i < 5; i++)
            {
                peakPoses.push_back(cv::Point2f((peakPoses[i].x + peakPoses[i + 1].x) / 2, 0));
            }
            std::sort(peakPoses.begin(), peakPoses.end(), [](auto& p1, auto& p2) { return p1.x < p2.x; });
            peakPoses.insert(peakPoses.begin(), cv::Point2f(peakPoses[0].x - 80, 0));
            peakPoses.push_back(cv::Point2f(peakPoses.back().x + 80, 0));

            Base::List<Math::Point2d> preExtracteds;
            for (int i = 0; i < peakPoses.size(); i++)
            {
                preExtracteds.push_back(Math::Point2d(peakPoses[i].x, peakPoses[i].y));
            }

            Geom::Line2d fitLineFirst;
            Geom::Line2d fitLineSecond;
            Geom::Line2d fitLineLastButOne;
            Geom::Line2d fitLineLast;
            int times = 3;
            for (int t = 0; t < times; t++)
            {
                if (extractedPoints.size() == 11)
                {
                    for (int i = 0; i < extractedPoints.size(); i++)
                    {
                        preExtracteds[i + 1].x() = extractedPoints[i].x();
                    }
                    extractedPoints.clear();
                }
                for (int i = 0; i < (int)preExtracteds.size() - 2; i++)
                {
                    float gapX1 = preExtracteds[i + 0].x();
                    float gapX2 = preExtracteds[i + 1].x();
                    float gapX3 = preExtracteds[i + 2].x();
                    int index1 = -1;
                    int index2 = -1;
                    int index3 = -1;
                    for (int p = 0; p < laserLine.size(); p++)
                    {
                        if (index1 == -1 && laserLine[p].x() > gapX1)
                        {
                            index1 = p;
                            continue;
                        }
                        if (index2 == -1 && laserLine[p].x() > gapX2)
                        {
                            index2 = p;
                            continue;
                        }
                        if (index3 == -1 && laserLine[p].x() > gapX3)
                        {
                            index3 = p;
                            break;
                        }
                    }

                    if (index1 != -1 && index2 - index1 > 10 && index3 - index2 > 10)
                    {
                        int index12;
                        int index23;
                        if ((i + 1) % 2 == 0)
                        {
                            index12 = index2 - (index2 - index1) / 4;
                            index23 = index2 + (index3 - index2) / 4;
                        }
                        else
                        {
                            index12 = index2;
                            index23 = index2;
                            index1 = index1 + (index2 - index1) / 4;
                            index3 = index3 - (index3 - index2) / 4;
                        }

                        if (i == 0)
                        {
                            index1 = index12 - 80;
                        }
                        else if (i + 3 == preExtracteds.size())
                        {
                            index3 = index23 + 80;
                        }

                        Base::List<Geom::Point2d> fitPoints1;
                        for (int p = index1; p < index12; p++)
                        {
                            fitPoints1.push_back(Geom::Point2d(laserLine[p].x(), laserLine[p].y()));
                        }
                        Geom::Line2d fitLine1;
                        CGAL::linear_least_squares_fitting_2(fitPoints1.begin(), fitPoints1.end(), fitLine1, CGAL::Dimension_tag<0>());
                        Base::List<Geom::Point2d> fitPoints2;
                        for (int p = index23; p < index3; p++)
                        {
                            fitPoints2.push_back(Geom::Point2d(laserLine[p].x(), laserLine[p].y()));
                        }
                        Geom::Line2d fitLine2;
                        CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                        if (t > 0)
                        {
                            for (auto iter = fitPoints1.begin(); iter != fitPoints1.end();)
                            {
                                if (CGAL::squared_distance(fitLine1, *iter) > 5 * 5)
                                {
                                    iter = fitPoints1.erase(iter);
                                }
                                else
                                {
                                    iter++;
                                }
                            }
                            CGAL::linear_least_squares_fitting_2(fitPoints1.begin(), fitPoints1.end(), fitLine1, CGAL::Dimension_tag<0>());

                            for (auto iter = fitPoints2.begin(); iter != fitPoints2.end();)
                            {
                                if (CGAL::squared_distance(fitLine2, *iter) > 5 * 5)
                                {
                                    iter = fitPoints2.erase(iter);
                                }
                                else
                                {
                                    iter++;
                                }
                            }
                            CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());
                        }

                        auto interV = CGAL::intersection(fitLine1, fitLine2);
                        Geom::Point2d interP;
                        if (CGAL::assign(interP, interV))
                        {
                            extractedPoints.push_back(Math::Point2d(interP.x(), interP.y()));
                        }

                        if (i == 0)
                        {
                            fitLineFirst = fitLine1;
                            fitLineSecond = fitLine2;
                        }
                        else if (i + 3 == preExtracteds.size())
                        {
                            fitLineLastButOne = fitLine1;
                            fitLineLast = fitLine2;
                        }
                    }
                }

                if (extractedPoints.size() != 11)
                {
                    return false;
                }
            }

            if (index == 4)
            {
                int index23 = laserLine.size() - 80;
                int index3 = laserLine.size() - 1;
                Base::List<Geom::Point2d> fitPoints2;
                for (int p = index23; p < index3; p++)
                {
                    fitPoints2.push_back(Geom::Point2d(laserLine[p].x(), laserLine[p].y()));
                }
                Geom::Line2d fitLine2;
                CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                for (auto iter = fitPoints2.begin(); iter != fitPoints2.end();)
                {
                    if (CGAL::squared_distance(fitLine2, *iter) > 5 * 5)
                    {
                        iter = fitPoints2.erase(iter);
                    }
                    else
                    {
                        iter++;
                    }
                }
                CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                auto interV = CGAL::intersection(fitLineLast, fitLine2);
                Geom::Point2d interP;
                if (CGAL::assign(interP, interV))
                {
                    extractedPoints.push_back(Math::Point2d(interP.x(), interP.y()));
                }
            }
            else if (index == 5)
            {
                int index23 = 0;
                int index3 = 80;
                Base::List<Geom::Point2d> fitPoints2;
                for (int p = index23; p < index3; p++)
                {
                    fitPoints2.push_back(Geom::Point2d(laserLine[p].x(), laserLine[p].y()));
                }
                Geom::Line2d fitLine2;
                CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                for (auto iter = fitPoints2.begin(); iter != fitPoints2.end();)
                {
                    if (CGAL::squared_distance(fitLine2, *iter) > 5 * 5)
                    {
                        iter = fitPoints2.erase(iter);
                    }
                    else
                    {
                        iter++;
                    }
                }
                CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                auto interV = CGAL::intersection(fitLineFirst, fitLine2);
                Geom::Point2d interP;
                if (CGAL::assign(interP, interV))
                {
                    extractedPoints.insert(extractedPoints.begin(), Math::Point2d(interP.x(), interP.y()));
                }
            }

            return true;

            Base::List<Geom::Point2d> fitPointsTop;
            Base::List<Geom::Point2d> fitPointsBot;
            for (int i = 1; i < (int)extractedPoints.size() - 1; i++)
            {
                if (i % 2 == 0)
                {
                    fitPointsTop.push_back(Geom::Point2d(extractedPoints[i].x(), extractedPoints[i].y()));
                }
                else
                {
                    fitPointsBot.push_back(Geom::Point2d(extractedPoints[i].x(), extractedPoints[i].y()));
                }
            }
            Geom::Line2d fitLineBot = Geom::Line2d(Geom::Point2d(extractedPoints[1].x(), extractedPoints[1].y()),
                Geom::Point2d(extractedPoints[3].x(), extractedPoints[3].y()));
            fitLineFirst = Geom::Line2d(Geom::Point2d(extractedPoints[0].x(), extractedPoints[0].y()),
                fitLineFirst.direction());
            auto interV = CGAL::intersection(fitLineFirst, fitLineBot);
            Geom::Point2d interP;
            if (CGAL::assign(interP, interV))
            {
                extractedPoints.insert(extractedPoints.begin(), Math::Point2d(interP.x(), interP.y()));
            }
            fitLineBot = Geom::Line2d(Geom::Point2d(extractedPoints[extractedPoints.size() - 2].x(), extractedPoints[extractedPoints.size() - 2].y()),
                Geom::Point2d(extractedPoints[extractedPoints.size() - 4].x(), extractedPoints[extractedPoints.size() - 4].y()));
            fitLineLast = Geom::Line2d(Geom::Point2d(extractedPoints[extractedPoints.size() - 1].x(), extractedPoints[extractedPoints.size() - 1].y()),
                fitLineLast.direction());
            interV = CGAL::intersection(fitLineLast, fitLineBot);
            if (CGAL::assign(interP, interV))
            {
                extractedPoints.push_back(Math::Point2d(interP.x(), interP.y()));
            }

            return extractedPoints.size() == 13;
        }


        inline bool extractSawtoothPoints(const Base::List<Math::Point2d>& laserLine, std::vector<Math::Point2d>& extractedPoints, int index, bool peakInMin)
        {
            if (peakInMin)
            {
                int fakeMode = Core::OiSettings()->value("FakeMode", 0).toInt();
                peakInMin = fakeMode == 0;
            }

            int gap = 280;

            int peakIndex = 0;
            float peakValue = peakInMin ? 2048 : -2048;
            int startIndex = 100;
            for (int i = startIndex; i < laserLine.size() && i < startIndex + gap; i++)
            {
                if (peakInMin)
                {
                    if (laserLine[i].y() < peakValue)
                    {
                        peakValue = laserLine[i].y();
                        peakIndex = i;
                    }
                }
                else
                {
                    if (laserLine[i].y() > peakValue)
                    {
                        peakValue = laserLine[i].y();
                        peakIndex = i;
                    }
                }
            }

            std::vector<Math::Point3f> peakPoses;
            peakPoses.push_back(Math::Point3f(laserLine[peakIndex].x(), laserLine[peakIndex].y(), peakIndex));

            startIndex = peakIndex + gap / 2;
            while (peakPoses.size() < 6 && startIndex + gap < laserLine.size())
            {
                int currPeakIndex = 0;
                peakValue = peakInMin ? 2048 : -2048;
                for (int i = startIndex; i < startIndex + gap; i++)
                {
                    if (peakInMin)
                    {
                        if (laserLine[i].y() < peakValue)
                        {
                            peakValue = laserLine[i].y();
                            currPeakIndex = i;
                        }
                    }
                    else
                    {
                        if (laserLine[i].y() > peakValue)
                        {
                            peakValue = laserLine[i].y();
                            currPeakIndex = i;
                        }
                    }
                }

                if (qAbs(laserLine[currPeakIndex].y() - peakPoses.back().y()) > 10)
                {
                    //break;
                }

                peakPoses.push_back(Math::Point3f(laserLine[currPeakIndex].x(), laserLine[currPeakIndex].y(), currPeakIndex));
                startIndex = currPeakIndex + gap / 2;
            }

            if (peakPoses.size() != 6)
            {
                return false;
            }

            for (int i = 0; i < 5; i++)
            {
                peakPoses.push_back(Math::Point3f((peakPoses[i].x() + peakPoses[i + 1].x()) / 2, 0, (peakPoses[i].z() + peakPoses[i + 1].z()) / 2));
            }
            std::sort(peakPoses.begin(), peakPoses.end(), [](auto& p1, auto& p2) { return p1.x() < p2.x(); });
            peakPoses.insert(peakPoses.begin(), Math::Point3f(peakPoses[0].x() - (peakPoses[1].x() - peakPoses[0].x()) / 2, 0, peakPoses[0].z() - 80));
            peakPoses.push_back(Math::Point3f(peakPoses.back().x() + (peakPoses.back().x() - peakPoses[peakPoses.size() - 2].x()) / 2, 0, peakPoses.back().x() + 80));

            Geom::Line2d fitLineFirst;
            Geom::Line2d fitLineSecond;
            Geom::Line2d fitLineLastButOne;
            Geom::Line2d fitLineLast;
            int times = 3;
            for (int t = 0; t < times; t++)
            {
                if (extractedPoints.size() == 11)
                {
                    for (int i = 0; i < extractedPoints.size(); i++)
                    {
                        peakPoses[i + 1].x() = extractedPoints[i].x();
                    }
                    extractedPoints.clear();
                }
                for (int i = 0; i < (int)peakPoses.size() - 2; i++)
                {
                    float gapX1 = peakPoses[i + 0].x();
                    float gapX2 = peakPoses[i + 1].x();
                    float gapX3 = peakPoses[i + 2].x();
                    int index1 = -1;
                    int index2 = -1;
                    int index3 = -1;
                    for (int p = 0; p < laserLine.size(); p++)
                    {
                        if (index1 == -1 && laserLine[p].x() > gapX1)
                        {
                            index1 = p;
                            continue;
                        }
                        if (index2 == -1 && laserLine[p].x() > gapX2)
                        {
                            index2 = p;
                            continue;
                        }
                        if (index3 == -1 && laserLine[p].x() > gapX3)
                        {
                            index3 = p;
                            break;
                        }
                    }

                    if (index1 != -1 && index2 - index1 > 10 && index3 - index2 > 10)
                    {
                        int index12;
                        int index23;
                        if ((i + 1) % 2 == 0)
                        {
                            index12 = index2 - (index2 - index1) / 4;
                            index23 = index2 + (index3 - index2) / 4;
                        }
                        else
                        {
                            index12 = index2;
                            index23 = index2;
                            index1 = index1 + (index2 - index1) / 4;
                            index3 = index3 - (index3 - index2) / 4;
                        }

                        if (i == 0)
                        {
                            index1 = index12 - 80;
                        }
                        else if (i + 3 == peakPoses.size())
                        {
                            index3 = index23 + 80;
                        }

                        Base::List<Geom::Point2d> fitPoints1;
                        for (int p = index1; p < index12; p++)
                        {
                            fitPoints1.push_back(Geom::Point2d(laserLine[p].x(), laserLine[p].y()));
                        }
                        Geom::Line2d fitLine1;
                        CGAL::linear_least_squares_fitting_2(fitPoints1.begin(), fitPoints1.end(), fitLine1, CGAL::Dimension_tag<0>());
                        Base::List<Geom::Point2d> fitPoints2;
                        for (int p = index23; p < index3; p++)
                        {
                            fitPoints2.push_back(Geom::Point2d(laserLine[p].x(), laserLine[p].y()));
                        }
                        Geom::Line2d fitLine2;
                        CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                        if (t > 0)
                        {
                            for (auto iter = fitPoints1.begin(); iter != fitPoints1.end();)
                            {
                                if (CGAL::squared_distance(fitLine1, *iter) > 5 * 5)
                                {
                                    iter = fitPoints1.erase(iter);
                                }
                                else
                                {
                                    iter++;
                                }
                            }
                            CGAL::linear_least_squares_fitting_2(fitPoints1.begin(), fitPoints1.end(), fitLine1, CGAL::Dimension_tag<0>());

                            for (auto iter = fitPoints2.begin(); iter != fitPoints2.end();)
                            {
                                if (CGAL::squared_distance(fitLine2, *iter) > 5 * 5)
                                {
                                    iter = fitPoints2.erase(iter);
                                }
                                else
                                {
                                    iter++;
                                }
                            }
                            CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());
                        }

                        auto interV = CGAL::intersection(fitLine1, fitLine2);
                        Geom::Point2d interP;
                        if (CGAL::assign(interP, interV))
                        {
                            extractedPoints.push_back(Math::Point2d(interP.x(), interP.y()));
                        }

                        if (i == 0)
                        {
                            fitLineFirst = fitLine1;
                            fitLineSecond = fitLine2;
                        }
                        else if (i + 3 == peakPoses.size())
                        {
                            fitLineLastButOne = fitLine1;
                            fitLineLast = fitLine2;
                        }
                    }
                }

                if (extractedPoints.size() != 11)
                {
                    return false;
                }
            }

            if (0)
            {
                if (index == 4)
                {
                    int index23 = laserLine.size() - 80;
                    int index3 = laserLine.size() - 1;
                    Base::List<Geom::Point2d> fitPoints2;
                    for (int p = index23; p < index3; p++)
                    {
                        fitPoints2.push_back(Geom::Point2d(laserLine[p].x(), laserLine[p].y()));
                    }
                    Geom::Line2d fitLine2;
                    CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                    for (auto iter = fitPoints2.begin(); iter != fitPoints2.end();)
                    {
                        if (CGAL::squared_distance(fitLine2, *iter) > 5 * 5)
                        {
                            iter = fitPoints2.erase(iter);
                        }
                        else
                        {
                            iter++;
                        }
                    }
                    CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                    auto interV = CGAL::intersection(fitLineLast, fitLine2);
                    Geom::Point2d interP;
                    if (CGAL::assign(interP, interV))
                    {
                        extractedPoints.push_back(Math::Point2d(interP.x(), interP.y()));
                    }
                }
                else if (index == 5)
                {
                    int index23 = 0;
                    int index3 = 80;
                    Base::List<Geom::Point2d> fitPoints2;
                    for (int p = index23; p < index3; p++)
                    {
                        fitPoints2.push_back(Geom::Point2d(laserLine[p].x(), laserLine[p].y()));
                    }
                    Geom::Line2d fitLine2;
                    CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                    for (auto iter = fitPoints2.begin(); iter != fitPoints2.end();)
                    {
                        if (CGAL::squared_distance(fitLine2, *iter) > 5 * 5)
                        {
                            iter = fitPoints2.erase(iter);
                        }
                        else
                        {
                            iter++;
                        }
                    }
                    CGAL::linear_least_squares_fitting_2(fitPoints2.begin(), fitPoints2.end(), fitLine2, CGAL::Dimension_tag<0>());

                    auto interV = CGAL::intersection(fitLineFirst, fitLine2);
                    Geom::Point2d interP;
                    if (CGAL::assign(interP, interV))
                    {
                        extractedPoints.insert(extractedPoints.begin(), Math::Point2d(interP.x(), interP.y()));
                    }
                }
            }

            return true;

            Base::List<Geom::Point2d> fitPointsTop;
            Base::List<Geom::Point2d> fitPointsBot;
            for (int i = 1; i < (int)extractedPoints.size() - 1; i++)
            {
                if (i % 2 == 0)
                {
                    fitPointsTop.push_back(Geom::Point2d(extractedPoints[i].x(), extractedPoints[i].y()));
                }
                else
                {
                    fitPointsBot.push_back(Geom::Point2d(extractedPoints[i].x(), extractedPoints[i].y()));
                }
            }

            //Geom::Line2d fitLineTop = Geom::Line2d(fitPointsTop.front(), fitPointsTop.back());
            ////Proc::FitLineD::execute(fitPointsTop, fitLineTop, 2);
            //auto interV = CGAL::intersection(fitLineSecond, fitLineTop);
            //Geom::Point2d interP;
            //if (CGAL::assign(interP, interV))
            //{
            //    extractedPoints[0] = Math::Point2d(interP.x(), interP.y());
            //}
            //
            //interV = CGAL::intersection(fitLineLastButOne, fitLineTop);
            //if (CGAL::assign(interP, interV))
            //{
            //    extractedPoints[10] = Math::Point2d(interP.x(), interP.y());
            //}

            Geom::Line2d fitLineBot = Geom::Line2d(Geom::Point2d(extractedPoints[1].x(), extractedPoints[1].y()),
                Geom::Point2d(extractedPoints[3].x(), extractedPoints[3].y()));
            fitLineFirst = Geom::Line2d(Geom::Point2d(extractedPoints[0].x(), extractedPoints[0].y()),
                fitLineFirst.direction());
            auto interV = CGAL::intersection(fitLineFirst, fitLineBot);
            Geom::Point2d interP;
            if (CGAL::assign(interP, interV))
            {
                extractedPoints.insert(extractedPoints.begin(), Math::Point2d(interP.x(), interP.y()));
            }
            fitLineBot = Geom::Line2d(Geom::Point2d(extractedPoints[extractedPoints.size() - 2].x(), extractedPoints[extractedPoints.size() - 2].y()),
                Geom::Point2d(extractedPoints[extractedPoints.size() - 4].x(), extractedPoints[extractedPoints.size() - 4].y()));
            fitLineLast = Geom::Line2d(Geom::Point2d(extractedPoints[extractedPoints.size() - 1].x(), extractedPoints[extractedPoints.size() - 1].y()),
                fitLineLast.direction());
            interV = CGAL::intersection(fitLineLast, fitLineBot);
            if (CGAL::assign(interP, interV))
            {
                extractedPoints.push_back(Math::Point2d(interP.x(), interP.y()));
            }

            return extractedPoints.size() == 13;
        }


        inline double pointToLineDistance(cv::Point2f* point, cv::Point2f* point1, cv::Point2f* point2)
        {
            return std::fabs((point2->y - point1->y) * point->x - (point2->x - point1->x) * point->y + point2->x * point1->y - point2->y * point1->x)
                / std::sqrt((point2->y - point1->y) * (point2->y - point1->y) + (point2->x - point1->x) * (point2->x - point1->x));
        }

        class CalibDlgImpl : public Impl<CalibDlg>
        {
        public:
            void init();
            void exit();
            static void refresh(const QMap<QString, Core::RangeMapPtr>& frame, const QDateTime& timestamp, void* data);
        public:
            QWidget* _toolbar;
            QToolButton* _calibration;
            QToolButton* _verification;
            QToolButton* _saveImageBtn;
            QStatusBar* _statusbar;
            QLabel* _statusText;
            CLTProfileView* _viewer;
            Appl::Chart* _chart;
            QMap<QString, Core::RangeMapPtr> _currentFrame;
            QTimer* _updateTimer;
            bool _autorange = true;
        };

        void CalibDlgImpl::init()
        {
            OI_Q(CalibDlg);


            QVBoxLayout* layout = new QVBoxLayout(q);

            _toolbar = new QWidget();
            _statusbar = new QStatusBar();

            QHBoxLayout* toolbarLayout = new QHBoxLayout(_toolbar);

            _calibration = new QToolButton(q);
            _calibration->setDefaultAction(new QAction(QIcon(":/resources/images/light/calibration.png"), CalibDlg::tr("一键标定")));
            _calibration->setIconSize(QSize(32, 32));
            _calibration->setToolButtonStyle(Qt::ToolButtonTextBesideIcon);
            _calibration->setStyleSheet("color:white");

            _verification = new QToolButton(q);
            _verification->setDefaultAction(new QAction(QIcon(":/resources/images/light/verification.png"), CalibDlg::tr("标块验证")));
            _verification->setIconSize(QSize(32, 32));
            _verification->setToolButtonStyle(Qt::ToolButtonTextBesideIcon);
            _verification->setStyleSheet("color:white");

            _saveImageBtn = new QToolButton(q);
            _saveImageBtn->setDefaultAction(new QAction(QIcon(":/resources/images/light/verification.png"), CalibDlg::tr("保存图像")));
            _saveImageBtn->setIconSize(QSize(32, 32));
            _saveImageBtn->setToolButtonStyle(Qt::ToolButtonTextBesideIcon);
            _saveImageBtn->setStyleSheet("color:white");

            toolbarLayout->addWidget(_calibration);
            toolbarLayout->addWidget(_verification);
            toolbarLayout->addWidget(_saveImageBtn);
            toolbarLayout->addStretch();

            _statusText = new QLabel();
            _statusbar->addWidget(_statusText);
            _viewer = new CLTProfileView(q);
            _chart = _viewer->chart();

            layout->addWidget(_toolbar);
            layout->addWidget(_viewer);
            layout->addWidget(_statusbar);

            QObject::connect(_calibration, &QToolButton::clicked, q, &CalibDlg::onCalibration);
            QObject::connect(_verification, &QToolButton::clicked, q, &CalibDlg::onVerification);
            QObject::connect(_saveImageBtn, &QToolButton::clicked, q, &CalibDlg::onSaveImage);

            q->setWindowTitle(CalibDlg::tr("数据标定&验证"));
            q->setGeometry(qApp->desktop()->availableGeometry().adjusted(34, 48, -24, 8));

            auto driver = LTCameraDriver::instance();
            driver->setExposure(LT360X::instance()->calibExposureTime());
            driver->setCallback(CalibDlgImpl::refresh, q);

            _updateTimer = new QTimer(q);
            _updateTimer->setSingleShot(false);
            _updateTimer->setInterval(100);
            QObject::connect(_updateTimer, &QTimer::timeout, q, &CalibDlg::onUpdateChart);

            Appl::ToolSeries* boundingBox = new Appl::ToolSeries();

            auto* setting = Core::OiSettings();
            int type = setting->value("CaptureType", 1).toInt();
            if (type == 1)
            {
                for (int i = 0; i < 101; i++)
                {
                    boundingBox->append(QPointF(LT360X::instance()->radius() * std::cos(CV_PI * 2 / 100 * i), LT360X::instance()->radius() * std::sin(CV_PI * 2 / 100 * i)));
                }
            }
            else
            {
                boundingBox->append(QPointF(-255, -27));
                boundingBox->append(QPointF(+255, -27));
                boundingBox->append(QPointF(+255, +27));
                boundingBox->append(QPointF(-255, +27));
                boundingBox->append(QPointF(-255, -27));
            }

            QPen pen(Qt::red);
            pen.setWidth(1);
            boundingBox->setPen(pen);
            _chart->addSeries(boundingBox);
            boundingBox->attachAxis(_chart->axisX());
            boundingBox->attachAxis(_chart->axisY());

            QMetaObject::invokeMethod(q, "onReady", Qt::QueuedConnection);
        }

        void CalibDlgImpl::exit()
        {
            OI_Q(CalibDlg);
            _updateTimer->stop();
            LTCameraDriver::instance()->stop();
            LTCameraDriver::instance()->setCallback(NULL, NULL);
            LTCameraDriver::instance()->setExposure(LT360X::instance()->exposureTime());
        }

        void CalibDlgImpl::refresh(const LTCameraFrame& frame, const QDateTime& timestamp, void* data)
        {
            auto q = static_cast<CalibDlg*>(data);
            QMetaObject::invokeMethod(q, "onRefresh", Qt::QueuedConnection, Q_ARG(const LTCameraFrame&, frame), Q_ARG(const QDateTime&, timestamp));
        }

        CalibDlg::CalibDlg(QWidget* parent /*= nullptr*/)
        {
            OI_I(CalibDlg)->init();
        }

        CalibDlg::~CalibDlg()
        {
			OI_E(CalibDlg)->exit();
        }

        void CalibDlg::onReady()
        {
            OI_F(CalibDlg);
            d->_updateTimer->start();
            LTCameraDriver::instance()->setMaxROI();
            LTCameraDriver::instance()->start();
        }

        void CalibDlg::onCalibration()
        {
            auto* setting = Core::OiSettings();
            int type = setting->value("CaptureType", 1).toInt();
            switch (type)
            {
            case 1: {
                calibModeB();
                break;
            }
            case 2: {
                calibSideWall();
                break;
            }
            default: {
                OiWarning() << "Unknown Capture Type onCalibration " << type;
                break;
            }
            }
        }

        void CalibDlg::onVerification()
        {
            OI_F(CalibDlg);
        }

        void CalibDlg::onSaveImage() const
        {
            OI_C(CalibDlg);

            QString appStr = QCoreApplication::applicationDirPath();
            QDir appDir(appStr);
            appDir.cdUp();
            appStr = appDir.absolutePath();
            auto keyArr = d->_currentFrame.keys();
            for (auto& k : keyArr)
            {
                QString imgDir = QString("%1/Data/Scanner/%2/%3").arg(appStr).arg(ProductParams::current()).arg(k);
                QDir dir(imgDir);
                if (!dir.exists())
                {
                    dir.mkpath(imgDir);
                    OiWarning() << "创建RangeMap目录：" << imgDir;
                }
                dir.setFilter(QDir::AllEntries | QDir::NoDotAndDotDot);
                int cnt = dir.count();
                Core::RangeMapPtr ptr = ThreadSafeMapOperator(d->_currentFrame, k);
                ptr->save(imgDir + "/" + QString::number(cnt) + ".mat");
            }
        }

        void CalibDlg::onFailImage() const
        {
            OI_C(CalibDlg);

            onSaveImage();

            OiAlerter() << "自动保存标定数据";
        }

        void CalibDlg::onRefresh(const LTCameraFrame& rangeMaps, const QDateTime& timestamp)
        {
            OI_F(CalibDlg);
            d->_currentFrame = rangeMaps;
        }

        void CalibDlg::onUpdateChart()
        {
            OI_F(CalibDlg);

            QList<QPointF> points;
            LT360X::instance()->rangeMapsToPoints(d->_currentFrame, points);
            if (points.size() > 0)
            {
                d->_chart->updateProfile(LTBase::pointCloudParser(points, LT360X::instance()->connectionThreshold(), LT360X::instance()->linkingThreshold(), LT360X::instance()->noiseThreshold()));
                if (d->_autorange)
                {
                    d->_chart->autoRange(0.8);
                    d->_autorange = false;
                }
            }
        }

        void CalibDlg::calibModeA()
        {
            OI_F(CalibDlg);
            if (QFileInfo::exists("../Setting/tempCalibrationData.xml") && QFileInfo("../Setting/tempCalibrationData.xml").isFile())
            {
                QFile file("../Setting/tempCalibrationData.xml");
                file.remove();
            }

            bool success = true;

            auto cameraNames = LT360X::instance()->cameraNames;

            for (int cameraIndex = 0; cameraIndex < cameraNames.size(); cameraIndex++)
            {
                int sampleSize = 1;

                std::vector<cv::Point2f> featurePoints;

                for (int loopIndex = 0; loopIndex < sampleSize; loopIndex++)
                {
                    auto rangemap = ThreadSafeMapOperator(d->_currentFrame, cameraNames[cameraIndex]);
                    if (!rangemap)
                    {
                        OiAlerter() << CalibDlg::tr("相机%1无轮廓数据!").arg(cameraIndex + 1);
                        OiWarning() << CalibDlg::tr("相机%1无轮廓数据!").arg(cameraNames[cameraIndex]);
                        return;
                    }

                    std::vector<Math::Point2d> points;
                    for (int i = 0; i < rangemap->cols(); i++)
                    {
                        if (rangemap->profile(0)[i] > 0)
                        {
                            auto undistortedPoint = LT360X::instance()->undistort(cameraNames[cameraIndex], Math::Point2d(i, rangemap->profile(0)[i]));
                            points.push_back(Math::Point2d(undistortedPoint.x(), undistortedPoint.y()));
                        }
                    }

                    std::vector<Math::Point2d> extractedPoints;
                    if (extractSawtoothPoints(points, extractedPoints, cameraIndex))
                    {
                        for (int i = 0; i < extractedPoints.size(); i++)
                        {
                            featurePoints.push_back(cv::Point2f(extractedPoints[i].x(), extractedPoints[i].y()));
                        }
                        break;
                    }
                }

                switch (cameraIndex)
                {
                case 0:
                case 1:
                case 2:
                case 3:
                case 6:
                case 7:
                    if (featurePoints.size() != 11)
                    {
                        OiAlerter() << "Camera " << cameraNames[cameraIndex] << CalibDlg::tr("轮廓提取失败!") << featurePoints.size();
                        OiWarning() << "Camera " << cameraNames[cameraIndex] << CalibDlg::tr("轮廓提取失败!") << featurePoints.size();
                        onFailImage();
                        return;
                    }
                    break;
                case 4:
                case 5:
                    if (featurePoints.size() != 12)
                    {
                        OiAlerter() << "Camera " << cameraNames[cameraIndex] << CalibDlg::tr("轮廓提取失败!") << featurePoints.size();
                        OiWarning() << "Camera " << cameraNames[cameraIndex] << CalibDlg::tr("轮廓提取失败!") << featurePoints.size();
                        onFailImage();
                        return;
                    }
                    break;
                }

                std::vector<cv::Point2f> targetPoints;
                cv::Point2f offset;

                double poleSize = 10;
                double gageSize = 20;

                switch (cameraIndex)
                {
                case 0:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(0 + gageSize / 2 * (i + 1), ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(0 + gageSize / 2 * i, (i % 2) * poleSize));
                    }
                    break;
                case 1:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(120 + gageSize / 2 * (i + 1), ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(120 + gageSize / 2 * i, (i % 2) * poleSize));
                    }
                    break;
                case 2:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(260 + gageSize / 2 * (i + 1), ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(260 + gageSize / 2 * i, (i % 2) * poleSize));
                    }
                    break;
                case 3:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(380 + gageSize / 2 * (i + 1), ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(380 + gageSize / 2 * i, (i % 2) * poleSize));
                    }
                    break;
                case 4:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(480 - gageSize / 2 * (i + 1), 25 - ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(480 - gageSize / 2 * i, 25 - (i % 2) * poleSize));
                    }
                    break;
                case 5:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(360 - gageSize / 2 * (i - 1), 25 - ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(360 - gageSize / 2 * i, 25 - (i % 2) * poleSize));
                    }
                    break;
                case 6:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(220 - gageSize / 2 * (i + 1), 25 - ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(220 - gageSize / 2 * i, 25 - (i % 2) * poleSize));
                    }
                    break;
                case 7:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(100 - gageSize / 2 * (i + 1), 25 - ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(100 - gageSize / 2 * i, 25 - (i % 2) * poleSize));
                    }
                    break;
                }
                offset = cv::Point2f(-240, -12.5);

                for (int i = 0; i < targetPoints.size(); i++)
                {
                    targetPoints[i] = targetPoints[i] + offset;
                }

                //cv::Mat finalTrans1 = cv::findHomography(featurePoints, targetPoints, cv::LMEDS);
                cv::Mat finalTrans1;
                if (featurePoints.size() != targetPoints.size())
                {
                    OiAlerter() << tr("标定错误：目标点与特征点不匹配");
                    onFailImage();
                    return;
                }
                else
                {
                    finalTrans1 = cv::findHomography(featurePoints, targetPoints);
                }

                //cv::Mat finalTrans = homoWithNelerMeadMin(featurePoints, targetPoints, 20);

                std::vector<cv::Point2f> featurePoints2;
                for (int i = 0; i < featurePoints.size(); i++)
                {
                    float rx = featurePoints[i].x;
                    float ry = featurePoints[i].y;
                    float denum = finalTrans1.at<double>(2, 0) * rx + finalTrans1.at<double>(2, 1) * ry + finalTrans1.at<double>(2, 2);
                    float x = (finalTrans1.at<double>(0, 0) * rx + finalTrans1.at<double>(0, 1) * ry + finalTrans1.at<double>(0, 2)) / denum;
                    float y = (finalTrans1.at<double>(1, 0) * rx + finalTrans1.at<double>(1, 1) * ry + finalTrans1.at<double>(1, 2)) / denum;

                    featurePoints2.push_back(cv::Point2f(x, y));
                }

                cv::Mat finalTrans2 = cv::findHomography(std::vector<cv::Point2f>(featurePoints2.begin() + 0, featurePoints2.begin() + 6),
                    std::vector<cv::Point2f>(targetPoints.begin() + 0, targetPoints.begin() + 6));
                //cv::Mat finalTrans3 = cv::findHomography(std::vector<cv::Point2f>(featurePoints2.begin() + 3, featurePoints2.begin() + 7),
                //    std::vector<cv::Point2f>(targetPoints.begin() + 3, targetPoints.begin() + 7));
                cv::Mat finalTrans3 = cv::findHomography(std::vector<cv::Point2f>(featurePoints2.begin() + 3, featurePoints2.begin() + 8),
                    std::vector<cv::Point2f>(targetPoints.begin() + 3, targetPoints.begin() + 8));
                cv::Mat finalTrans4 = cv::findHomography(std::vector<cv::Point2f>(featurePoints2.begin() + 5, featurePoints2.end()),
                    std::vector<cv::Point2f>(targetPoints.begin() + 5, targetPoints.end()));
                int gapX1 = suitGap(featurePoints[3], featurePoints[4], targetPoints[3], targetPoints[4], finalTrans1, finalTrans2, finalTrans3);
                int gapX2 = suitGap(featurePoints[6], featurePoints[7], targetPoints[6], targetPoints[7], finalTrans1, finalTrans3, finalTrans4);

                cv::Mat finalTransWithMove1 = cv::Mat(3, 4, CV_64FC1);
                finalTrans1.copyTo(finalTransWithMove1.colRange(0, 3));
                finalTransWithMove1.at<double>(0, 3) = gapX1;
                finalTransWithMove1.at<double>(1, 3) = gapX2;
                finalTransWithMove1.at<double>(2, 3) = 2000;
                cv::Mat finalTransWithMove2 = cv::Mat::zeros(3, 4, CV_64FC1);
                cv::Mat finalTransWithMove3 = cv::Mat::zeros(3, 4, CV_64FC1);
                cv::Mat finalTransWithMove4 = cv::Mat::zeros(3, 4, CV_64FC1);
                cv::Mat finalTransWithMove5 = cv::Mat::zeros(3, 4, CV_64FC1);
                finalTrans2.copyTo(finalTransWithMove2.colRange(0, 3));
                finalTrans3.copyTo(finalTransWithMove3.colRange(0, 3));
                finalTrans4.copyTo(finalTransWithMove4.colRange(0, 3));
                //finalTrans5.copyTo(finalTransWithMove5.colRange(0, 3));

                cv::FileStorage opencv_file("../Setting/tempCalibrationData.xml", cv::FileStorage::APPEND);
                opencv_file << LT360X::instance()->cameraParams[cameraIndex * 5 + 0].toStdString() << finalTransWithMove1;
                opencv_file << LT360X::instance()->cameraParams[cameraIndex * 5 + 1].toStdString() << finalTransWithMove2;
                opencv_file << LT360X::instance()->cameraParams[cameraIndex * 5 + 2].toStdString() << finalTransWithMove3;
                opencv_file << LT360X::instance()->cameraParams[cameraIndex * 5 + 3].toStdString() << finalTransWithMove4;
                //opencv_file << LT360X::instance()->cameraParams[cameraIndex * 5 + 4].toStdString() << finalTransWithMove5;
                opencv_file.release();
            }

            if (success)
            {
                QFile oldFile("../Setting/NewCalibrationData.xml");
                oldFile.rename(QString("../Setting/NewCalibrationData%1.xml").arg(QDateTime::currentDateTime().toString("_yyyy-MM-dd-hh-mm-ss-zzz")));
                QFile newFile("../Setting/tempCalibrationData.xml");
                newFile.rename("../Setting/newCalibrationData.xml");

                OiAlerter() << CalibDlg::tr("标定完成!");

                LT360X::instance()->loadHomography();
            }
        }

        void CalibDlg::calibModeB()
        {
            OI_F(CalibDlg);

            if (QFileInfo::exists("../Setting/tempCalibrationData.xml") && QFileInfo("../Setting/tempCalibrationData.xml").isFile())
            {
                QFile file("../Setting/tempCalibrationData.xml");
                file.remove();
            }

            bool success = true;

            auto cameraNames = LT360X::instance()->cameraNames;
            for (int cameraIndex = 0; cameraIndex < cameraNames.size(); cameraIndex++)
            {
                int sampleSize = 1;

                int poleAmount = 5;

                std::vector<int> index;

                double noiseThreshold = 5;
                double connectionThreshold = 10;
                double linkingThreshold = 20;

                std::vector<cv::Point2f> featurePoints;
                std::vector<cv::Point2f> calibrationTargetOrdered;

                int correctSamples = 0;

                for (int loopIndex = 0; loopIndex < sampleSize; loopIndex++)
                {
                    auto rangemap = d->_currentFrame[cameraNames[cameraIndex]];
                    if (!rangemap)
                    {
                        OiAlerter() << tr("无轮廓数据!");
                        return;
                    }
                    QList<QPointF> points;
                    int takeProfile = LT360X::instance()->takeProfile(rangemap);

                    for (int i = 0; i < rangemap->cols(); i++)
                    {
                        if (rangemap->profile(takeProfile)[i] > 0)
                        {
                            auto undistortedPoint = LT360X::instance()->undistort(cameraNames[cameraIndex], Math::Point2d(i, rangemap->profile(takeProfile)[i]));
                            points.append(QPointF(undistortedPoint.x(), undistortedPoint.y()));
                        }
                    }
                    QList<QList<QPointF>> outputQList = Math::profileParser(points, connectionThreshold, linkingThreshold, noiseThreshold);

                    int shrinkingWindow = 1;

                    QList<QList<QPointF>> hahalist;
                    for (int i = 0; i < outputQList.size(); i++)
                    {
                        if (std::fabs(outputQList[i][0].x() - outputQList[i][outputQList[i].size() - 1].x()) > 50)
                        {
                            hahalist.append(outputQList[i]);

                        }
                    }
                    outputQList = hahalist;

                    if (outputQList.size() != poleAmount)
                    {
                        if (outputQList.size() == poleAmount + 1)
                        {
                            if (cameraIndex % 2 == 0)
                            {
                                outputQList.erase(outputQList.begin() + 2);
                            }
                            else
                            {
                                outputQList.erase(outputQList.begin() + 3);
                            }
                        }
                        else
                        {
                            success = false;
                            continue;
                        }
                    }

                    if (outputQList.size() == poleAmount)
                    {
                        correctSamples++;

                        for (int i = 0; i < poleAmount; i++)
                        {
                            double maxError = -1;
                            int maxErrorPos;
                            double distance;
                            for (int j = 0; j < outputQList[i].size(); j++)
                            {
                                distance = pointToLineDistance(&cv::Point2f(outputQList[i][j].rx(), outputQList[i][j].ry()), &cv::Point2f(outputQList[i][0].rx(), outputQList[i][0].ry()), &cv::Point2f(outputQList[i][outputQList[i].size() - 1].rx(), outputQList[i][outputQList[i].size() - 1].ry()));
                                if (distance > maxError)
                                {
                                    maxError = distance;
                                    maxErrorPos = j;
                                }
                            }

                            if (maxError < -1)//too lazy to delete but i love commenting
                            {
                                outputQList[i + 1] = outputQList[i] + outputQList[i + 1];
                            }
                            else
                            {
                                std::vector<cv::Point2f> Laser1;
                                std::vector<cv::Point2f> Laser2;

                                for (int j = shrinkingWindow; j < maxErrorPos - shrinkingWindow; j++)
                                {
                                    Laser1.push_back(cv::Point2f(outputQList[i][j].ry(), outputQList[i][j].rx()));
                                }

                                for (int j = maxErrorPos + shrinkingWindow; j < outputQList[i].size() - shrinkingWindow; j++)
                                {
                                    Laser2.push_back(cv::Point2f(outputQList[i][j].ry(), outputQList[i][j].rx()));
                                }

                                cv::Vec4f line1, line2;
                                cv::fitLine(cv::Mat(Laser1), line1, CV_DIST_HUBER, 0, 0.01, 0.01);
                                cv::fitLine(cv::Mat(Laser2), line2, CV_DIST_HUBER, 0, 0.01, 0.01);

                                cv::Point2f x = cv::Point2f(line2[2], line2[3]) - cv::Point2f(line1[2], line1[3]);
                                cv::Point2f d1 = cv::Point2f(line1[0], line1[1]);
                                cv::Point2f d2 = cv::Point2f(line2[0], line2[1]);

                                double cross = d1.x * d2.y - d1.y * d2.x;
                                double t1 = (x.x * d2.y - x.y * d2.x) / cross;
                                cv::Point2f intersectionPoint = cv::Point2f(line1[2], line1[3]) + d1 * t1;
                                featurePoints.push_back(cv::Point2f(intersectionPoint.y, intersectionPoint.x));
                            }
                        }
                    }
                }

                if (!success)
                {
                    OiAlerter() << LT360X::instance()->cameraParams[cameraIndex] << tr("无轮廓数据!");
                    onFailImage();
                    return;
                }

                std::vector<cv::Point2f> targetPoints;
                cv::Point2f offset;

                double poleSize = LT360X::instance()->poleSize();
                double gageSize = LT360X::instance()->gageSize();
                double peakDist = (gageSize - poleSize) * 0.5;
                double peakPosi = peakDist / qSqrt(2);

                switch (cameraIndex)
                {
                case 0:
                    for (int i = 0; i < sampleSize; i++)
                    {
                        targetPoints.push_back(cv::Point2f(-peakDist, peakDist));
                        targetPoints.push_back(cv::Point2f(0, peakDist));
                        targetPoints.push_back(cv::Point2f(-peakDist, -peakDist));
                        targetPoints.push_back(cv::Point2f(0, -peakDist));
                        targetPoints.push_back(cv::Point2f(peakDist, -peakDist));
                    }
                    offset = cv::Point2f(-(poleSize * 0.5), -(poleSize * 0.5));// +cv::Point2f(50, 50);
                    break;
                case 1:
                    for (int i = 0; i < sampleSize; i++)
                    {
                        targetPoints.push_back(cv::Point2f(peakDist, peakDist));
                        targetPoints.push_back(cv::Point2f(0, peakDist));
                        targetPoints.push_back(cv::Point2f(-peakDist, peakDist));
                        targetPoints.push_back(cv::Point2f(0, -peakDist));
                        targetPoints.push_back(cv::Point2f(-peakDist, -peakDist));


                    }
                    offset = cv::Point2f(-(poleSize * 0.5), (poleSize * 0.5));// +cv::Point2f(50, 50);
                    break;
                case 2:
                    for (int i = 0; i < sampleSize; i++)
                    {
                        targetPoints.push_back(cv::Point2f(peakDist, -peakDist));
                        targetPoints.push_back(cv::Point2f(0, -peakDist));
                        targetPoints.push_back(cv::Point2f(peakDist, peakDist));
                        targetPoints.push_back(cv::Point2f(0, peakDist));
                        targetPoints.push_back(cv::Point2f(-peakDist, peakDist));
                    }
                    offset = cv::Point2f((poleSize * 0.5), (poleSize * 0.5));// +cv::Point2f(50, 50);
                    break;
                case 3:
                    for (int i = 0; i < correctSamples; i++)
                    {
                        targetPoints.push_back(cv::Point2f(-peakDist, -peakDist));
                        targetPoints.push_back(cv::Point2f(0, -peakDist));
                        targetPoints.push_back(cv::Point2f(peakDist, -peakDist));
                        targetPoints.push_back(cv::Point2f(0, peakDist));
                        targetPoints.push_back(cv::Point2f(peakDist, peakDist));
                    }
                    offset = cv::Point2f((poleSize * 0.5), -(poleSize * 0.5));// +cv::Point2f(50, 50);
                    break;
                }

                for (int i = 0; i < targetPoints.size(); i++)
                {
                    targetPoints[i] = targetPoints[i] + offset;
                }
                cv::Mat finalTrans = cv::findHomography(featurePoints, targetPoints);

                cv::FileStorage opencv_file("../Setting/tempCalibrationData.xml", cv::FileStorage::APPEND);
                opencv_file << LT360X::instance()->cameraParams[cameraIndex].toStdString() << finalTrans;
                opencv_file.release();
            }

            if (success)
            {
                QFile oldFile("../Setting/NewCalibrationData.xml");
                oldFile.rename(QString("../Setting/NewCalibrationData%1.xml").arg(QDateTime::currentDateTime().toString("_yyyy-MM-dd-hh-mm-ss-zzz")));
                QFile newFile("../Setting/tempCalibrationData.xml");
                newFile.rename("../Setting/newCalibrationData.xml");

                LT360X::instance()->loadHomography();
            }
        }

        void CalibDlg::calibSideWall()
        {
            OI_F(CalibDlg);

            if (QFileInfo::exists("../Setting/tempCalibrationData.xml") && QFileInfo("../Setting/tempCalibrationData.xml").isFile())
            {
                QFile file("../Setting/tempCalibrationData.xml");
                file.remove();
            }

            bool success = true;

            auto cameraNames = LT360X::instance()->cameraNames;

            for (int cameraIndex = 0; cameraIndex < cameraNames.size(); cameraIndex++)
            {
                int sampleSize = 1;

                std::vector<cv::Point2f> featurePoints;

                for (int loopIndex = 0; loopIndex < sampleSize; loopIndex++)
                {
                    auto rangemap = ThreadSafeMapOperator(d->_currentFrame,cameraNames[cameraIndex]);
					if (!rangemap || rangemap->rows() == 0)
                    {
                        OiAlerter() << tr("相机%1无轮廓数据!").arg(cameraIndex + 1);
                        onFailImage();
                        return;
                    }

                    std::vector<Math::Point2d> points;
                    for (int i = 0; i < rangemap->cols(); i++)
                    {
                        float* pArr = rangemap->profile(0);
                        if (pArr == nullptr)
                        {
                            continue;
                        }
                        float val = pArr[i];
                        if (val > 0)
                        {
							auto undistortedPoint = LT360X::instance()->undistort(cameraNames[cameraIndex], Math::Point2d(i, val));
                            if (!Math::IsNan(undistortedPoint.x()))
							{
								points.push_back(undistortedPoint);
                            }
                        }
                    }

                    if (points.empty())
                    {
                        OiWarning() << "Range map points is empty " << cameraNames[cameraIndex];
                        OiAlerter() << "Range map points is empty " << cameraNames[cameraIndex];
                        onFailImage();
                        return;
                    }
                    else
                    {

                        std::vector<Math::Point2d> extractedPoints;
                        if (extractSawtoothPoints(points, extractedPoints, cameraIndex, LT360X::instance()->driver() == "Fake"))
                        {
                            for (int i = 0; i < extractedPoints.size(); i++)
                            {
                                featurePoints.push_back(cv::Point2f(extractedPoints[i].x(), extractedPoints[i].y()));
                            }
                            break;
                        }

                    }
                }

                int homoIndex = cameraNames[cameraIndex].right(1).toInt() - 1;
                switch (homoIndex)
                {
                case 0:
                case 1:
                case 2:
                case 3:
                case 6:
                case 7:
                    if (featurePoints.size() != 11)
                    {
                        OiAlerter() << "Camera " << cameraNames[cameraIndex] << CalibDlg::tr("轮廓提取失败!") << featurePoints.size();
                        OiWarning() << "Camera " << cameraNames[cameraIndex] << CalibDlg::tr("轮廓提取失败!") << featurePoints.size();
                        onFailImage();
                        return;
                    }
                    break;
                case 4:
                case 5:
                    if (featurePoints.size() != 11)
                        //if (featurePoints.size() != 12)
                    {
                        OiAlerter() << "Camera " << cameraNames[cameraIndex] << CalibDlg::tr("轮廓提取失败!") << featurePoints.size();
                        OiWarning() << "Camera " << cameraNames[cameraIndex] << CalibDlg::tr("轮廓提取失败!") << featurePoints.size();
                        onFailImage();
                        return;
                    }
                    break;
                }

                std::vector<cv::Point2f> targetPoints;
                cv::Point2f offset;

                double poleSize = 10;
                double gageSize = 20;

                switch (homoIndex)
                {
                case 0:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(0 + gageSize / 2 * (i + 1), ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(0 + gageSize / 2 * i, (i % 2) * poleSize));
                    }
                    break;
                case 1:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(120 + gageSize / 2 * (i + 1), ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(120 + gageSize / 2 * i, (i % 2) * poleSize));
                    }
                    break;
                case 2:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(260 + gageSize / 2 * (i + 1), ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(260 + gageSize / 2 * i, (i % 2) * poleSize));
                    }
                    break;
                case 3:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(380 + gageSize / 2 * (i + 1), ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(380 + gageSize / 2 * i, (i % 2) * poleSize));
                    }
                    break;
                case 4:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(480 - gageSize / 2 * (i + 1), 25 - ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(480 - gageSize / 2 * i, 25 - (i % 2) * poleSize));
                    }
                    break;
                case 5:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(360 - gageSize / 2 * (i - 1), 25 - ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(360 - gageSize / 2 * i, 25 - (i % 2) * poleSize));
                    }
                    break;
                case 6:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(220 - gageSize / 2 * (i + 1), 25 - ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(220 - gageSize / 2 * i, 25 - (i % 2) * poleSize));
                    }
                    break;
                case 7:
                    for (int i = 0; i < featurePoints.size(); i++)
                    {
                        //targetPoints.push_back(cv::Point2f(100 - gageSize / 2 * (i + 1), 25 - ((i + 1) % 2) * poleSize));
                        targetPoints.push_back(cv::Point2f(100 - gageSize / 2 * i, 25 - (i % 2) * poleSize));
                    }
                    break;
                }
                offset = cv::Point2f(-240, -12.5);

                for (int i = 0; i < targetPoints.size(); i++)
                {
                    targetPoints[i] = targetPoints[i] + offset;
                }

                cv::Mat finalTrans = cv::findHomography(featurePoints, targetPoints);
                //cv::Mat finalTrans = cv::estimateRigidTransform(featurePoints, targetPoints, true);
                //cv::Mat finalTrans2 = cv::estimateAffine2D(featurePoints, targetPoints);
                //cv::Mat finalTrans3 = cv::estimateAffinePartial2D(featurePoints, targetPoints);
                cv::FileStorage opencv_file("../Setting/tempCalibrationData.xml", cv::FileStorage::APPEND);
                opencv_file << LT360X::instance()->cameraParams[homoIndex].toStdString() << finalTrans;
                opencv_file.release();
            }

            if (success)
            {
                QFile oldFile("../Setting/NewCalibrationData.xml");
                oldFile.rename(QString("../Setting/NewCalibrationData%1.xml").arg(QDateTime::currentDateTime().toString("_yyyy-MM-dd-hh-mm-ss-zzz")));
                QFile newFile("../Setting/tempCalibrationData.xml");
                newFile.rename("../Setting/newCalibrationData.xml");

                OiAlerter() << tr("标定完成!");

                LT360X::instance()->loadHomography();
            }
        }

    }
}
